/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
* This is an effort to test if the Occupancygrid can be used for SLAMing
*/ 

#include<stdio.h>
#include<stdlib.h>
#include<math.h>
#include "CScanMatchInterface.h" ///< Scanmatch interface
#include "CAHistogramECorrelationScanMatch.h" ///< Scanmatch 
#include "TOccupancyGrid.h" ///< Occupancy grid class
#include "glutImageGUI.h"   ///< For visualisation
#include "COccuMCL.h"
/// For reading the data from file
int readScanLine181(FILE *f,ScanMatch::scan &meas, ScanMatch::pose &p){
		float time_s;
		int i;
		char c;
		fscanf(f,"%f %f %f",&p.x,&p.y,&p.a); ///< Pose
	//fprintf(stderr,"1 ");
		for(i=0;i<181;i++){
				fscanf(f,"%f %f",&meas.r[i],&meas.a[i]);
				fscanf(f,"%c",&c);
   // fprintf(stderr,"%.1f ",meas.r[i]);
				if(c=='\n' && i<181){
						fprintf(stderr,"EOL reached at i=%d\n",i);
						meas.N = i;
						return 0;
				}
		}
	//fprintf(stderr,"3 \n");
		meas.N = 181;
		return 0;
}

void scan2absoluteXY( ScanMatch::scan &s, float *x, float *y, ScanMatch::pose &pose){
	int i;
	if(x==NULL || y==NULL) return;
	for(i=0;i<s.N;i++){
		x[i]=s.r[i]*cos(s.a[i]+pose.a)+pose.x;
		y[i]=s.r[i]*sin(s.a[i]+pose.a)+pose.y;
	}
}

float minMSD(float *x,float *y, float px,float py, int N, int &mini, float &dx2, float &dy2){
	float dist2=100000;
	float d2;
	int minInd = 0;
	float dx,dy;
	for(int i=0;i<N;i++){
		dx = (x[i]-px)*(x[i]-px);
		dy = (y[i]-py)*(y[i]-py);
		d2 = dx + dy;
		if(d2<dist2){ 
			dist2 = d2;
			dx2 = dx;
			dy2 = dy;
			minInd = i;
		}
	}
	mini = minInd;
	return dist2;
}

float computeMSE(ScanMatch::scan &s1,ScanMatch::scan &s2,ScanMatch::pose &p1,ScanMatch::pose &p2, 
							float &x_err ,float &y_err)
{
	float *x1 = (float *) malloc(s1.N*sizeof(float));  
	float *y1 = (float *) malloc(s1.N*sizeof(float));
	float *x2 = (float *) malloc(s2.N*sizeof(float));  
	float *y2 = (float *) malloc(s2.N*sizeof(float));

	long int ss1,us1,ss2,us2,sec,us,ms;
	ownTime_GetTimeOfDay(&ss1,&us1);

	scan2absoluteXY(s1,x1,y1,p1);
	scan2absoluteXY(s2,x2,y2,p2);
	
	int mini;
	float dist=0,d;
	float dx2=0,dy2=0,ddx,ddy;
	int num=0;
	for(int i=0;i<s1.N;i++){
		if(s1.r[i] <= 0 || s2.r[i]<=0) continue;
		d = minMSD(x2,y2,x1[i],y1[i],s2.N,mini,ddx,ddy);
		if(d<1.0){
			dist += d; 
			dx2 += ddx;
			dy2 += ddy;
			num++;
		}
	}	

	ownTime_GetTimeOfDay(&ss2,&us2);
	sec = ss2-ss1;us = us2-us1;ms = sec*1000+us/1000;
	float fms=(float)sec*1000.0+(float)us/1000.0;
	if(num!=0){
		x_err = sqrt(dx2/num);
		y_err = sqrt(dy2/num);
	}
	else{
		x_err = -666;
		y_err = -666;
	}
	free(x1);
	free(x2);
	free(y1);
	free(y2);
	
	if(num!=0){ 
		return dist/num;
	}
	return -666; 
}


void setRGBpix(unsigned char *rgb,int i,int j, int Nx, unsigned char r,unsigned char g, unsigned char b ){
	rgb[3*Nx*j+3*i]=r;
	rgb[3*Nx*j+3*i+1]=g;
	rgb[3*Nx*j+3*i+2]=b;
}

void setRGBmap(unsigned char *rgb,TOccupancyGrid *gr, unsigned char r,unsigned char g, unsigned char b){
	float *data = gr->getDataPtr();
	int Nx,Ny;
	gr->getDim(Nx,Ny);
	
	for(int j=0;j<Ny;j++){
		for(int i=0;i<Nx;i++){
			setRGBpix(rgb,i,j, Nx, r*data[Nx*j+i],g*data[Nx*j+i],b*data[Nx*j+i]);
		}
	}	
}

void drawLine(unsigned char *RGB, int Nx,int Ny,float resolution,   ///Grid parameters
							uint pix, uint piy,float angle, float length, ///< Start pixel, angle to draw and linelength
							unsigned char r,unsigned char g,unsigned char b){
		float dy = sin(angle);
		float dx = cos(angle);
		float rr=0;
		int i=0;
		uint x,y;
		while(rr<length){
				x = pix + i*dx;
				y = piy - i*dy;
				RGB[3*Nx*y+3*x] = r;
				RGB[3*Nx*y+3*x+1] = g;
				RGB[3*Nx*y+3*x+2] = b;
				i++;
				rr+=resolution;
		}
}


int filterScan(ScanMatch::scan &m, Grid::TScan &g){
		int bad=0;
		float dist_th=1.0;
		float dr_plus,dr_minus;
		
		for(int i=1;i<m.N-1;i++){
				dr_plus = fabs(m.r[i]-m.r[i+1]);
				dr_minus = fabs(m.r[i]-m.r[i-1]);
				if(dr_plus>dist_th && dr_minus>dist_th) g.r[i] = -1.0; 
		}
		for(int i=0;i<m.N;i++){
				g.a[i] = m.a[i];
				if(g.r[i] == -1){ g.r[i]=0; bad++;} 
				else g.r[i] = m.r[i];
				if(g.r[i] > 35){g.r[i] = 0; bad++;}///Filter out long distances
		}
		return bad;
}


int main(void){
		float grid_size = 45.0;
		TOccupancyGrid O(grid_size,grid_size,0.05,Grid::TCoord2D(0,0));
		COccuMCL omcl;
		grid_t *origmap;
		grid_t *dataPtr;
		int Nx,Ny;
		int oNx,oNy;
		unsigned char *rgb;
		FILE *log = fopen("j2b2_measurement.txt","rt");
		// ../../res/autmap-new.txt
		/*
		omcl.initialize("labra.map",
										4000, 
										mcl::pose(1,-1,-0*M_PI/180), //mcl::pose(2,-3,-20*M_PI/180), 
										mcl::pose(1.0,1.0,M_PI/8),
										15,
										-M_PI/2.0,
										0.8);
		*/
		omcl.initialize("../../res/autmap-new.txt",
										1.0, 
										2000, 
										mcl::pose(10,-5,-10*M_PI/180), 
										mcl::pose(1.0,1.0,M_PI/8),
										15,
										-M_PI/2.0,
										0.1,
										1.0);
		
		omcl.O->getDim(oNx,oNy);
		fprintf(stderr,"Map size = %dx%d\n",oNx,oNy);
		origmap = (grid_t *) malloc(oNx*oNy*sizeof(float));
		dataPtr = omcl.O->getDataPtr();
		memcpy(origmap,dataPtr,oNx*oNy*sizeof(float));		
				
		mcl::pose mpos,mest;
		mcl::scan mvirt;
		mvirt.allocate(181);
		
		
		///Scanmatch stuff
		CScanMatchInterface *gmatcher; ///< This is for global	
		gmatcher = new CAHistogramECorrelationScanMatch(
																						.6,   // Search area in meters 
																						0.6);     // Search angle in radians
		
		ScanMatch::pose pos,odo_cur,odo_ref,est;   ///< The estimated position
		ScanMatch::scan meas,ref,meas0;  ///< The Reference scan 
		meas.allocate(181);
		ref.allocate(181);
		meas0.allocate(181);		
		///Initialize grid as empty
 		O.setGridToValue(0.5);
		O.getDim(Nx,Ny);
		
		///Initialize Image display
		rgb = (unsigned char *) malloc(3*Nx*Ny);
		unsigned char *orgb = (unsigned char *) malloc(3*oNx*oNy);
		addImageWindow(Nx, Ny);
		addImageWindow(oNx, oNy);
		glutImageGUI_Start();
		
		Grid::TScan s,virt; ///< for occupancy grid
		s.alloc(181);
		virt.alloc(181);
		readScanLine181(log,meas0,odo_cur);
	
		odo_cur.set(0,0,0);
		O.setScanFillProb(odo_cur.x, odo_cur.y ,odo_cur.a, *((Grid::TScan *) &meas0),0.01,0.99);
		O.fillRGB(rgb,1.0/255.0);
		displayImage(0, rgb,Ny,Nx);
		
		long int ss0,us0,ss1,us1,ss2,us2,sec,us,ms;
		float fms;
		ownTime_GetTimeOfDay(&ss0,&us0);
		int cnt=0;
		odo_ref = odo_cur;
    fprintf(stderr,"ref.N=%d meas.N=%d  \n",ref.N,meas.N);
		
		float updatesincedist=1000;
		mcl::pose g_origin;
		mcl::pose g_global_origin;
		vector<mcl::pose> orig;
		vector<mcl::pose> orig_est;
		vector<float> weights;
		bool updateOrigin = true;
		bool origin_updated = false;
		bool remap = false;
		while(!feof(log)){
				readScanLine181(log,meas,odo_cur);
				if(feof(log)){
				  remap = true;
				}
				meas.filt(25.0);
				////////////////////////////////////////////////////////////////////////////
				/// The SLAM
				////////////////////////////////////////////////////////////////////////////
				
				est = est.calculateDifferentialSensorMovement(odo_ref,odo_cur, 0);
				
				pos = pos.integrateDifferential(est);
				O.getScan(pos.x,pos.y, (pos.a-M_PI/2.0), 181, M_PI/180.0, (grid_t) 0.7, virt); ///<getscan
				for(int i=0;i<meas.N;i++){
						ref.a[i]=virt.a[i]-M_PI/2.0;
						ref.r[i]=virt.r[i]; 
						if (virt.r[i]<0)ref.r[i]=0;
				}	
				gmatcher->setCurrentPose(pos);
				gmatcher->setReferenceScan(ref,pos); 
				gmatcher->update(meas,pos);    ///update
				gmatcher->getCurrentPose(pos); ///< Get the estimate			
				
				////////////////////////////////////////////////////////////////////////////
				/// The MCL
				////////////////////////////////////////////////////////////////////////////
				//mest.setToDifferentialPose(  *((mcl::pose *) &odo_cur),*((mcl::pose *) &odo_ref));
				mest.setToDifferentialPose(  *((mcl::pose *) &odo_cur),*((mcl::pose *) &odo_ref));
				omcl.runMCL(mest, mcl::pose(fabs(est.x)*0.02+0.01,fabs(est.y)*0.02+0.01
						,0.005*fabs(est.a)),  *((mcl::scan *) &meas));
				odo_ref = odo_cur;
				
				mcl::pose mp = omcl.filt.getDistributionMean(); ///< The distribution mean
				mp.to2PI();
				/*
				mcl::pose mp(0,0,0); // = omcl.filt.getDistributionMean(); ///< The distribution mean
				int num=0;
				for(int i=0;i<omcl.filt.NumOfParticles;i++){
					if( omcl.filt.Particles[i].p > (omcl.prob_max-.01/omcl.filt.NumOfParticles)){
						mp.x+=omcl.filt.Particles[i].x;
						mp.y+=omcl.filt.Particles[i].y;
						mp.a+=omcl.filt.Particles[i].a;
						num++;
					}
				}
				mp.x /=num;mp.y /=num;mp.a /=num;
				*/
				//fprintf(stderr,"Lik=%.3f g_o:(%.2f,%.2f,%.2f) mp:(%.2f,%.2f,%.2f)\n",
				//				omcl.filt.Lik, g_origin.x,g_origin.y,g_origin.a,
				//			 mp.x,mp.y,mp.a);
				
				
				////////////////////////////////////////////////////////////////////////////
				/// The ReMapping
				////////////////////////////////////////////////////////////////////////////
				float toPos = sqrt(pos.x*pos.x+pos.y*pos.y);
				float toEdge;
				toEdge = (grid_size/2.0)-toPos;			
				if(toEdge < 15.0 || orig.size()>500){
					fprintf(stderr,"Invoking the remapping!!\n");
					remap = true;
				}
				
 				if(omcl.filt.Lik>200){
					mcl::pose up_pos; up_pos.set(0,0,0);					
					up_pos.setToDifferentialPose(mcl::pose(0,0,0),*((mcl::pose *) &pos));
				//	fprintf(stderr,"up_pos:(%.2f,%.2f,%.2f) mp:(%.2f,%.2f,%.2f) or:(%.2f,%.2f,%.2f)",
				//					     up_pos.x,up_pos.y,up_pos.a,mp.x,mp.y,mp.a, g_origin.x,g_origin.y,g_origin.a);
					mcl::pose g_origin_tmp = mp.integrateDifferential(up_pos); 
					orig.push_back(g_origin_tmp);
					weights.push_back(omcl.filt.Lik);
					g_origin.set(0,0,0);
					float w_sum = 0;
					for(int i=0;i<orig.size();i++){
						//g_origin.x+=weights[i]*orig[i].x/orig.size();
						//g_origin.y+=weights[i]*orig[i].y/orig.size();
						//g_origin.a+=weights[i]*orig[i].a/orig.size();
						g_origin.x+=orig[i].x/orig.size();
						g_origin.y+=orig[i].y/orig.size();
						g_origin.a+=orig[i].a/orig.size();
						
						w_sum += weights[i];
					}
					orig_est.push_back(g_origin);
					//g_origin.x /= w_sum;
					//g_origin.y /= w_sum;
					//g_origin.a /= w_sum;
					
					fprintf(stderr,"L:=%.2f, Sum=%.3f pos:(%.2f,%.2f,%.2f) up_pos:(%.2f,%.2f,%.2f) mp:(%.2f,%.2f,%.2f) or:(%.2f,%.2f,%.2f) or_tmp:(%.2f,%.2f,%.2f)\n",omcl.filt.Lik,w_sum,pos.x,pos.y,pos.a,
									up_pos.x,up_pos.y,up_pos.a,mp.x,mp.y,mp.a, g_origin.x,g_origin.y,g_origin.a,
									g_origin_tmp.x,g_origin_tmp.y,g_origin_tmp.a);
					
					origin_updated = true;
				}
				
			//	if((O.information_sum>15000 && omcl.filt.Lik>30 && orig.size()>0) ||
			//					(orig.size()>20 && O.information_sum>20000) ){
				//if(origin_updated || remap){
				if(remap){
					origin_updated = false;
					Grid::TCoord2D coord;
					grid_t v;
					float x,y,l,a;
					memcpy(dataPtr,origmap,oNx*oNy*sizeof(float));		
					O.information_sum = 0;
					float err_l=0,err_a=0;
					//for(int i=0;i<orig_est.size()-1;i++){
					mcl::pose up_pos;
					up_pos.setToDifferentialPose(orig_est[orig_est.size()-1],orig_est[0]);
					err_l += sqrt(up_pos.x*up_pos.x+up_pos.y*up_pos.y);
					err_a += fabs(up_pos.a);
					//}
					fprintf(stderr,"ERROR l=%.2f a=%.2f\n",err_l,err_a);
					while(err_a<0) err_a+=M_PI*2.0;
					while(err_a>M_PI) err_a-=M_PI*2.0;
					
					if(orig.size()>10 && err_l<0.4 && fabs(err_a)<5*M_PI/180){
						for(int j=0;j<Ny;j++){
								for(int i=0;i<Nx;i++){
								O.getValue(Grid::TPixel(i,j),v);
								if(v == 0.5) continue;
								coord = O.toCoord(Grid::TPixel(i,j));
								l = sqrt(coord.x*coord.x + coord.y*coord.y);
								a = atan2(coord.y,coord.x);
								x=l*cos(a+g_origin.a) + g_origin.x;
								y=l*sin(a+g_origin.a) + g_origin.y;
								coord.x = x; coord.y=y;
								if(!remap){
										v -= 0.22; if(v<0.25) v=0.25;
										omcl.O->updateProbability(coord, v);
								}
								else{
										omcl.O->updateProbability(coord, v);
								}
								
							}
						}
					}
					if(remap){
							if(orig.size()>10){
									memcpy(origmap,dataPtr,oNx*oNy*sizeof(float));
									omcl.setOccupiedValue(0.9);
									omcl.updateDt();
							}
							remap = false;
							O.setGridToValue(0.5);
							orig.clear();
							orig.push_back(mp);
							weights.clear();
							orig_est.clear();
							weights.push_back(omcl.filt.Lik);
							pos.set(0,0,0); ///< initialixe
							O.setScanFillProb(0, 0 ,0, *((Grid::TScan *) &meas),0.01,0.99);
					}
					updatesincedist = 0;
					//omcl.updateDt();
					//orig.clear();
					//for(int i =0;i<10;i++) orig.push_back(g_origin);
				}
				
				
				
				////////////////////////////////////////////////////////////////////////////
				/// Error
				////////////////////////////////////////////////////////////////////////////
				
				/*
				O.getScan(pos.x,pos.y, (pos.a-M_PI/2.0), 181, M_PI/180.0, (grid_t) 0.8, virt); ///<getscan
				
				for(int i=0;i<meas.N;i++){
					ref.a[i]=virt.a[i]-M_PI/2.0;
					ref.r[i]=virt.r[i]; 
					if(virt.r[i]<0)virt.r[i]=0;
				}
				float de,xe,ye;
				de = computeMSE(meas,ref,pos,pos,xe ,ye);
				*/								
				////////////////////////////////////////////////////////////////////////////
				/// Visualisation
				////////////////////////////////////////////////////////////////////////////
				/// SLAM
				
				filterScan(meas,s);
				//O.setScanFillProb(pos.x, pos.y ,pos.a,*((Grid::TScan *) &meas) ,0.3,0.7);
				O.setScanFillProb(pos.x, pos.y ,pos.a,s,0.3,0.9);
				
				if(cnt%10==0 || remap){
					//fprintf(stdout,"Drawing");
					O.fillRGB(rgb,1.0/255.0);
					displayImage(0, rgb,Ny,Nx);
					
					/// MCL
					
					setRGBmap(orgb,omcl.O, 0,255, 0);
					Grid::TCoord2D coord;
					Grid::TPixel pix;
					for(int i=0;i<omcl.filt.NumOfParticles;i++){
						coord.x = omcl.filt.Particles[i].x;
						coord.y = omcl.filt.Particles[i].y;
						pix = omcl.O->toPixel(coord);
						setRGBpix(orgb,pix.x,pix.y, oNx, 255,255, 0);
					}
					
					omcl.getScan(mvirt, mp);
					omcl.getScan(mvirt, mp);
					for(int i=0;i<mvirt.N;i++){
						coord.x = mp.x + mvirt.r[i]*cos(mvirt.a[i]+mp.a);
						coord.y = mp.y + mvirt.r[i]*sin(mvirt.a[i]+mp.a);
						pix = omcl.O->toPixel(coord);
						if(pix.x>=0 && pix.x<oNx&&pix.y>=0&&pix.y<oNy)
							setRGBpix(orgb,pix.x,pix.y, oNx, 255,0, 0);
					}
					
					//omcl.getScan(mvirt, g_origin);
					/// Draw the first measurement into the estimated origin
					/*
					for(int i=0;i<mvirt.N;i++){
						coord.x = g_origin.x + meas0.r[i]*cos(meas0.a[i]+g_origin.a);
						coord.y = g_origin.y + meas0.r[i]*sin(meas0.a[i]+g_origin.a);
						pix = omcl.O->toPixel(coord);
						if(pix.x>=0 && pix.x<oNx&&pix.y>=0&&pix.y<oNy)
							setRGBpix(orgb,pix.x,pix.y, oNx, 255,0, 0);
					}
					*/
					
					for(int i=0;i<meas.N;i++){
						coord.x = mp.x + meas.r[i]*cos(meas.a[i]+mp.a);
						coord.y = mp.y + meas.r[i]*sin(meas.a[i]+mp.a);
						pix = omcl.O->toPixel(coord);
						if(pix.x>=0 && pix.x<oNx&&pix.y>=0&&pix.y<oNy)
							setRGBpix(orgb,pix.x,pix.y, oNx, 0,255, 255);
					}
					Grid::TCoord2D ocoord;
					ocoord.x = g_origin.x;
					ocoord.y = g_origin.y;
					Grid::TPixel opix = omcl.O->toPixel(ocoord);
					drawLine(orgb, oNx,oNy, omcl.O->getResolution(),opix.x,opix.y,g_origin.a, 2.0,255,0,0);
					drawLine(orgb, oNx,oNy, omcl.O->getResolution(),opix.x,opix.y,g_origin.a+M_PI/2, 2.00,0,255,0);
					ocoord.x = mp.x;ocoord.y = mp.y;
					opix = omcl.O->toPixel(ocoord);
					drawLine(orgb, oNx,oNy, omcl.O->getResolution(),opix.x,opix.y,mp.a, 2.0,255,0,0);
					drawLine(orgb, oNx,oNy, omcl.O->getResolution(),opix.x,opix.y,mp.a+M_PI/2, 2.0,0,255,0);
				
					///Origin
					/*
					ocoord.x = 0;ocoord.y = 0;
					opix = omcl.O->toPixel(ocoord);	
					drawLine(orgb, oNx,oNy, omcl.O->getResolution(),opix.x,opix.y,0, 5.0,255,0,0);
					drawLine(orgb, oNx,oNy, omcl.O->getResolution(),opix.x,opix.y,M_PI/2, 5.0,255,255,0);
					*/
					displayImage(1, orgb,oNy,oNx);
					
				}
				////////////////////////////////////////////////////////////////////////////
				/// The end
				////////////////////////////////////////////////////////////////////////////
				
				cnt++;
		}
		
		ownTime_GetTimeOfDay(&ss2,&us2);
		sec = ss2-ss0;us = us2-us0; ms = sec*1000+us/1000;
		fms=(float)sec*1000.0+(float)us/1000.0;
		fprintf(stderr,"TOTAL::(%.4f) = (%.4f)/scan \n",fms,fms/cnt);
		
		fprintf(stderr,"The End - press ctrl C to stop\n");
		while(1) usleep(1000*1000);
		
		return 0;
}